//*************************************************************************************************
//**************************************M DULO DE ENCODER******************************************
//*************************************************************************************************
//Proyecto: AMARANTA
//Autor: Carlos Felipe Santacruz Rosero
//Fecha de Modificaci n: Septiembre 25, 2007
//Descripci n:
// a = Kp + Ki +Kd
// b = - (Kp + 2*Kd)
// c = Kd


#include <p30f6010.h>    
#include <ports.h>

//******************************DEFINICI N DE CONSTANTES**********************************
#define NUM_ENCODER 1             // N mero de encoders por pata 
#define REL_REDUCTOR_531 531     //  Relaci n del reductor del motor
#define PINON_MOT 48             // N mero de dientes del pi  n del eje del motor 
#define PINON_RUEDA 24           // N mero de dientes del pi on del eje de la llanta
#define TM 0.01                  // Tiempo de muestreo del control 


//********************************VARIABLES GLOBALES***************************************
float encoder/*[NUM_ENCODER]*/;
float encoder_aux/*[NUM_ENCODER]*/;
float encoder_past/*[NUM_ENCODER]*/;
float encoder_vel_aux/*[NUM_ENCODER]*/;
float encoder_vel/*[NUM_ENCODER]*/;
unsigned int control_encoder/*[NUM_ENCODER]*/;

//void encoders(void);
//************************************RUTINAS************************************************
// Rutina de inicio del encoder:
void vAmarantaEncodersInit(void)
{
	char i;
	TRISEbits.TRISE8	= 1;
	TRISEbits.TRISE9	= 1;
	TRISAbits.TRISA14	= 1;
	TRISAbits.TRISA15	= 1;
	
	//for (i=0;i<NUM_ENCODER;i++)
	//	encoder[i] = encoder_aux[i] = control_encoder[i] = encoder_past[i]= encoder_vel[i] = encoder_vel_aux[i] = 0;
	encoder=encoder_aux=control_encoder=encoder_past=encoder_vel=encoder_vel_aux=0;
	ConfigINT1(RISING_EDGE_INT & EXT_INT_PRI_7 & EXT_INT_ENABLE);
}

// Rutina que calcula la velocidad de la llanta tomando los conteos del encoder:
void vAmarantaEncoders(void)
{  	
	// Normalizar los conteos de una vuelta del motor-reductor as  de m s de una vuelta:
	while (encoder >= 191160)
		encoder -= 191160;
	
	while (encoder < 0)
		encoder += 191160;
	
	// Calcular en grados la rotaci n de la llanta:
	encoder_aux     = (encoder/REL_REDUCTOR_531)*(PINON_MOT/PINON_RUEDA);  // [0,720)                      
	
	// Normalizar el  ngulo entre [0 360):
	while (encoder_aux >= 360)
		encoder_aux -= 360;
	
	
	while (encoder_aux < 0)
		encoder_aux += 360;	
	
	// Calcular la velocidad en grados/s
	encoder_vel_aux =  (encoder_aux - encoder_past)/TM;  // Rango Vel : [-117,117] grados/seg
	
	// Verificar que la velocidad est  en el rango permitido y no se salga por discontinuidad del dominio del  ngulo:
	if ( -130 <=encoder_vel && encoder_vel <= 130 ) 
	{ 
		encoder_vel = encoder_vel_aux;                      // Variable que se usa para el controlador de velocidad
		control_encoder = (unsigned int) encoder_vel;       // Variable para ver la velocidad en el hyperterminal:
		encoder_past = encoder_aux ;                        // Actualizar el valor de la posici n para el pr ximo c lculo.
	}
}

// Rutina de interrupci n del encoder: manejo de conteos del encoder:
// (No agregar nada m s en la rutina de interrupci n porque el tiempo es cr tico)
void __attribute__((__interrupt__)) _INT1Interrupt(void)
{
	if (PORTDbits.RD0) 
		encoder[0] += 1;
	else 
		encoder[0] -= 1;

	IFS1bits.INT1IF = 0;
}

